#include "pid.h"

void pid_init(pid_struct_t *pid,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max)
{
  pid->kp      = kp;
  pid->ki      = ki;
  pid->kd      = kd;
  pid->i_max   = i_max;
  pid->out_max = out_max;
}
extern pid_count angle_pid;
extern pid_count speed_pid;
float pid_calc(pid_struct_t *pid, float ref, float fdb)
{
  pid->ref = ref;
  pid->fdb = fdb;
  pid->err[1] = pid->err[0];
  pid->err[0] = pid->ref - pid->fdb;

  pid->p_out  = pid->kp * pid->err[0];
  pid->i_out += pid->ki * pid->err[0];
  pid->d_out  = pid->kd * (pid->err[0] - pid->err[1]);
  LIMIT_MIN_MAX(pid->i_out, -pid->i_max, pid->i_max);

  pid->output = pid->p_out + pid->i_out + pid->d_out;
  LIMIT_MIN_MAX(pid->output, -pid->out_max, pid->out_max);
  return pid->output;
}
void update_angle(motor_angle* _angle,uint16_t angle_fb)
{
	_angle->encoder = angle_fb;
	if(_angle->encoder_is_init)
	{
		if(_angle->encoder-_angle->last_encoder>4096)
		_angle->round_cnt --;
		if(_angle->encoder-_angle->last_encoder<-4096)
		_angle->round_cnt ++;
	}
	else
	{
		_angle->encoder_offset =_angle->encoder;
		_angle->encoder_is_init=1;
	}
	_angle->angle_offset = _angle->encoder_offset/8192.0f*360.0f;
	_angle->last_encoder = _angle->encoder;
	_angle->total_encoder=_angle->round_cnt*8192+_angle->encoder-_angle->encoder_offset;
	_angle->angle =_angle->total_encoder/8192.0f*360.0f;
}
